/*
Copyright (C) 2003 Michael Würtinger

This file is part of Dynamicengine.

Dynamicengine is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation, either version 3
of the License, or (at your option) any later version.

Dynamicengine is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Dynamicengine. If not, see http://www.gnu.org/licenses/.
*/


#include "include.h"
#include "main.h"
#include "uvec.h"
#include "physics.h"

CSite::CSite()
{
	gravity[0] = 0.0;
	gravity[1] = -9.81;
	gravity[2] = 0.0;

	friction = 1.0;
	ground_friction = 15.0;
}

CSite::~CSite()
{
}

void CSite::Simulate(double t)
{
	static std::list<CTruss*>::iterator t_start, t_end, t_iter;
	t_start = trusses.begin();
	t_end = trusses.end();
	t_iter = t_start;

	while(t_iter != t_end)
	{
		if(*t_iter)
		{
			static vec3f current_truss;    //Vector representing the current Truss
			static float current_length;   //Current Truss' length

			//Calculate the Vector defining this Truss. 
			current_truss = (*t_iter)->joints[0]->r - (*t_iter)->joints[1]->r;
			
			//Calculate the current Truss' length.
			current_length = current_truss.length();

			//Here the Force which affects this Truss is calculated
			(*t_iter)->stress = (current_length - (*t_iter)->length) *  (*t_iter)->d;

			//This Force is multiplicated with the current Truss' vector, so that you get the Force's Vector!
			(*t_iter)->F = (*t_iter)->stress * current_truss;

			if((*t_iter)->Break())
			{
				t_iter--;
				break;
			}


		}

		t_iter++; //GoTo the next Truss
	}	
	
	
	static std::list<CJoint*>::iterator j_start, j_end, j_iter;
	j_start = joints.begin();
	j_end = joints.end();
	j_iter = j_start;

	while(j_iter != j_end)
	{
		if(*j_iter)
		{
			static vec3f vF_M; //
			
			vF_M[0] = vF_M[1] = vF_M[2] = 0;  //clear the vector (necessary, as 'static')


			//Run through the trusses array.
			for(int i=0; i < MAX_TRUSSES; i++)
			{
				if((*j_iter)->trusses[i] != NULL)  //Only use valid trusses!
				{
					//Check whether this joint is the first or second in the trusses' array
					if((*j_iter) == (*j_iter)->trusses[i]->joints[0])
						vF_M += -(*j_iter)->trusses[i]->F;  //If it's the first, we have to invert the Force Vector
					else
						vF_M += (*j_iter)->trusses[i]->F;  //If it's the second, the Vector has the right direction

				}
			}

			//Here first the acceleration is calculated. Then the gravity is added and the whole term	is multiplicated with the time span. The resulting speed is added to the current Joint speed.
			(*j_iter)->v += ((vF_M / (*j_iter)->m) + gravity) * t;

/* !!!!! */	(*j_iter)->v *= ( 1 - (*j_iter)->friction * friction * t);
			
			//Now the Joint's position is updated by the current speed multiplicated by the time span.
			(*j_iter)->r += (*j_iter)->v * t;

			
			//Make sure	that Joints keep in the site's area!
			
			if((*j_iter)->r[0] > XSIZE)
			{
				(*j_iter)->r[0] = XSIZE;
				(*j_iter)->v[0] = 0.0;
			}
			if((*j_iter)->r[0] < -XSIZE)
			{
				(*j_iter)->r[0] = -XSIZE;
				(*j_iter)->v[0] = 0.0;
			}

			if((*j_iter)->r[1] > YSIZE)
			{
				(*j_iter)->r[1] = YSIZE;
				(*j_iter)->v[1] = 0.0;
/* !!! */		//(*j_iter)->v[0] *= (1 - ground_friction * t);
			}
			if((*j_iter)->r[1] < -YSIZE)
			{
				(*j_iter)->r[1] = -YSIZE;
				(*j_iter)->v[1] = 0.0;
/* !!! */		//(*j_iter)->v[0] *= (1 - ground_friction * t);
			}
		}

		j_iter++; //GoTo the next Joint
	}
}

CTruss* CSite::CreateTruss(CJoint* joint1, CJoint* joint2, CMaterial* material)
{
	if(!joint1 || !joint2 || !material)
	{
		////MessageBox(NULL, "CSite::CreateTruss(): The Engine tried to create an Truss with an invalid Joint pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return NULL;
	}

	CTruss* pTruss = new CTruss(this, joint1, joint2, material);
	
	trusses.push_front(pTruss);

	return pTruss;
}

CJoint* CSite::CreateJoint(bool bStatic, vec3f pos, vec3f velocity, float friction)
{
	CJoint* pJoint = new CJoint(this, pos, velocity, friction);
	
	if(bStatic)
		static_joints.push_front(pJoint);
	else
		joints.push_front(pJoint);
	
	return pJoint;
}

void CSite::Clear()
{
	static std::list<CTruss*>::iterator t_start, t_end, t_iter;
	t_start = trusses.end();
	t_end = trusses.begin();
	t_iter = t_start;

	while(t_iter != t_end)
	{
		if(*t_iter)
		{
			delete (*t_iter);
		}

		t_iter++; //GoTo the next Truss
	}

	static std::list<CJoint*>::iterator sj_start, sj_end, sj_iter;
	sj_start = static_joints.begin();
	sj_end = static_joints.end();
	sj_iter = sj_start;

	while(sj_iter != sj_end)
	{
		if(*sj_iter)
		{
			delete (*sj_iter);
		}

		sj_iter++; //GoTo the next Joint
	}

	static std::list<CJoint*>::iterator j_start, j_end, j_iter;
	j_start = joints.begin();
	j_end = joints.end();
	j_iter = j_start;

	while(j_iter != j_end)
	{
		if(*j_iter)
		{
			delete (*j_iter);
		}

		j_iter++; //GoTo the next Joint
	}

	
	trusses.clear();
	joints.clear();
	static_joints.clear();
}

void CSite::RemoveTruss(CTruss* pTruss)
{
	trusses.remove(pTruss);
	delete pTruss;
}

void CSite::RemoveJoint(CJoint* pJoint)
{
	joints.remove(pJoint);
	static_joints.remove(pJoint);

	delete pJoint;
}

CJoint* CSite::IsJoint(float x, float y)
{
	static std::list<CJoint*>::iterator j_start, j_end, j_iter;
	j_start = joints.begin();
	j_end = joints.end();
	j_iter = j_start;

	while(j_iter != j_end)
	{
		if(*j_iter)
		{
			if((*j_iter)->r[0] == x && (*j_iter)->r[1] == y)
				return *j_iter;
		}

		j_iter++;
	}


	static std::list<CJoint*>::iterator sj_start, sj_end, sj_iter;
	sj_start = static_joints.begin();
	sj_end = static_joints.end();
	sj_iter = sj_start;

	while(sj_iter != sj_end)
	{
		if(*sj_iter)
		{
			if((*sj_iter)->r[0] == x && (*sj_iter)->r[1] == y)
				return *sj_iter;
		}

		sj_iter++;
	}


	return NULL;
}

void CSite::Draw()
{
	glColor3f(0.2, 0.2, 0.2);

	glBegin(GL_LINES);
	for(float i = -YSIZE; i<=YSIZE; i+=DISTANCE)
	{
		glVertex2f(-XSIZE, i);
		glVertex2f( XSIZE, i);
	}
	for(float i=-XSIZE; i<=XSIZE; i+=DISTANCE)
	{
		glVertex2f(i, -YSIZE);
		glVertex2f(i,  YSIZE);
	}
	glEnd();
	
	
	//
	// Draw Trusses
	//
	
	static std::list<CTruss*>::iterator t_start, t_end, t_iter;
	t_start = trusses.begin();
	t_end = trusses.end();
	t_iter = t_start;

	glBegin(GL_LINES);
	glColor3f(0.0, 1.0, 0.0);
	
	while(t_iter != t_end)
	{
		if(*t_iter)
		{
			glColor3fv((*t_iter)->color());

			glVertex3f((*t_iter)->joints[0]->r[0], (*t_iter)->joints[0]->r[1], (*t_iter)->joints[0]->r[2]);
			glVertex3f((*t_iter)->joints[1]->r[0], (*t_iter)->joints[1]->r[1], (*t_iter)->joints[1]->r[2]);
		}

		t_iter++;
	}

	glEnd();


/*	//
	// Draw Joints
	//
	
	static std::list<CJoint*>::iterator j_start, j_end, j_iter;
	j_start = joints.begin();
	j_end = joints.end();
	j_iter = j_start;

	glBegin(GL_POINTS);
	glColor3f(1.0, 1.0, 1.0);
	
	while(j_iter != j_end)
	{
		if(*j_iter)
		{
			glVertex3f((*j_iter)->r[0], (*j_iter)->r[1], (*j_iter)->r[2]);
		}

		j_iter++;
	}

	glEnd();*/

	
	//
	// Draw Static Joints
	//

	static std::list<CJoint*>::iterator sj_start, sj_end, sj_iter;
	sj_start = static_joints.begin();
	sj_end = static_joints.end();
	sj_iter = sj_start;

	glBegin(GL_POINTS);
	glColor3f(1.0, 0.0, 0.0);
	
	while(sj_iter != sj_end)
	{
		if(*sj_iter)
		{
			glVertex3f((*sj_iter)->r[0], (*sj_iter)->r[1], (*sj_iter)->r[2]);
		}

		sj_iter++;
	}

	glEnd();
}

//
// CTruss
//

CTruss::CTruss(CSite* Site, CJoint* joint1, CJoint* joint2, CMaterial* material)
{
	if(!Site)
	{
		////MessageBox(NULL, "CTruss::CTruss(): The Engine tried to create an Truss with an invalid Site pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return;
	}

	pSite = Site;

	if((!joint1) || (!joint2))
	{
		////MessageBox(NULL, "CTruss::CTruss(): The Engine tried to create an Truss with an invalid Joint pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return;
	}


	joints[0] = joint1;
	joints[1] = joint2;

	
	length		 = (joints[0]->r - joints[1]->r).length();
	
	d			 = material->GetD(length);
	m			 = material->GetMass(length);
	max_pressure = material->GetMaxPressure();
	max_pull	 = material->GetMaxPull();

	F = vec3f(0, 0, 0);
	stress = 0.0;

	//cerr << d <<" "<< m <<" "<< max_pressure <<" "<< max_pull <<"\n";

	joint1->AddTruss(this);
	joint2->AddTruss(this);
}

CTruss::~CTruss()
{
	//joints[0]->RemoveTruss(this);
	//joints[1]->RemoveTruss(this);
}

/*
bool RemoveFromJoint(CJoint* joint)
{
	if(joint != joints[0] && joint != joints[1])
	{
		////MessageBox(NULL, "The Engine tried to remove an Truss from an Joint with an invalid pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return false;
	}
	
	for(int i=0; i < MAX_TRUSSES; i++)
	{
		if(joint->trusses[i] == this)
		{
			trusses[i] = NULL;

			return true;
		}
	}

	return false;
}*/

float* CTruss::color()
{
	static float c[3];
	static float p;
	
	if(stress < 0.0)  // Pressure
	{
		p = (-stress) / max_pressure;

		if(p > 1.0)
			p = 1.0;

		c[0] = sqrt(p);//sin(p * PI / 2);
		c[1] = (1 - sqrt(p)) * 2;//cos(p * PI / 2);
		c[2] = 0.0;

		return c;
	}
	if(stress > 0.0)  // Pull
	{
		p = stress / max_pull;
		
		if(p > 1.0)
			p = 1.0;

		c[0] = 0.0;
		c[1] = (1 - sqrt(p)) * 2;
		c[2] = sqrt(p);

		return c;
	}

	c[0] = 0.0;
	c[1] = 1.0;
	c[2] = 0.0;

	return c;
}

bool CTruss::Break()
{
	if(-stress > max_pressure || stress > max_pull)
	{
		joints[0]->RemoveTruss(this);
		joints[1]->RemoveTruss(this);
		pSite->RemoveTruss(this);
		return true;
	}

	return false;
}

//
// CJoint
//

CJoint::CJoint(CSite* Site, vec3f pos, vec3f velocity, float _friction)
{
	if(!Site)
	{
		////MessageBox(NULL, "CJoint::CJoint(): The Engine tried to create an Joint with an invalid Site pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return;
	}

	pSite = Site;
	
	
	r = pos;
	v = velocity;
		
	friction = _friction;

	m = 0.0;

	for(int i=0; i < MAX_TRUSSES; i++)
		trusses[i] = NULL;	
}

CJoint::~CJoint()
{
	for(int i=0; i < MAX_TRUSSES; i++)
	{
		if(trusses[i])
		{
			if(trusses[i]->joints[0] == this)
				trusses[i]->joints[0] = NULL;
			if(trusses[i]->joints[1] == this)
				trusses[i]->joints[1] = NULL;
		}
	}
}

bool CJoint::AddTruss(CTruss* truss)
{
	if(!truss)
	{
		////MessageBox(NULL, "CJoint::AddTruss(): The Engine tried to add an Truss with an invalid pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return false;
	}

	
	for(int i=0; i < MAX_TRUSSES; i++)
	{
		if(trusses[i] == NULL)
		{
			trusses[i] = truss;
			//printf("Truss Mass: %f\n", truss->m);
			m += (truss->m / 2);
			return true;
		}
	}

	////MessageBox(NULL, "CJoint::AddTruss(): The Engine couldn't add a Truss to a Joint, because the MAX_TRUSSES limit was reched!", "Engine", MB_ICONHAND);
	app_exit(0);
	return false;
}

bool CJoint::RemoveTruss(CTruss* truss)
{
	if(!truss)
	{
		////MessageBox(NULL, "RemoveTruss(): The Engine tried to remove an Truss with an invalid pointer!", "Engine", MB_ICONHAND);
		app_exit(0);
		return false;
	}

//	if(truss->joints[0] == this)
//		truss->joints[0] = NULL;
//	if(truss->joints[1] == this)
//		truss->joints[1] = NULL;

	
	for(int i=0; i < MAX_TRUSSES; i++)
	{
		if(trusses[i] == truss)
		{
			trusses[i] = NULL;
			return true;
		}
	}

	return false;
}
